#!/home/aixiaoxiao/miniconda3/envs/yolov8/bin/python
# -*- coding: utf-8 -*-

# 此程序用于实现视频分帧识别物体,并为所识别的物品添加矩形框，显示置信度、标签等，更新于2024/6/24
# 更新程序，用于显示实时三维坐标2024/6/24
# PWMFeedback: ['#021P1461!', '#022P0995!', '#023P2222!', '#024P0818!', '#025P1500!']

# [0.09189158511750145, -1.1898782175471343, 1.7011724219188729, 1.6069246423111792, 0.0]


import json
import time
import cv2
import numpy as np
import pyrealsense2 as rs#type: ignore  # 将YOLOv8导入到该py文件中
from ultralytics import YOLO#type: ignore  # 将YOLOv8导入到该py文件中
from scipy.spatial.transform import Rotation as R#type: ignore  # 将YOLOv8导入到该py文件中

# 加载官方或自定义模型
model = YOLO("best13.pt")  # 加载一个官方的检测模型

# 深度相机配置
pipeline = rs.pipeline()  # 定义流程pipeline，创建一个管道
config = rs.config()  # 定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)  # 初始化摄像头深度流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
pipe_profile = pipeline.start(config)  # 启用管段流
align = rs.align(rs.stream.color)  # 这个函数用于将深度图像与彩色图像对齐

# Park算法的标定参数（end_link->camera）
translation = np.array([-0.0214793, -0.0486306, -0.0450709])  # x,y,z (m)
quaternion = [0.9956458782, -0.0894632558, 0.0188356071, 0.0181887597]  # w,x,y,z

# base_link -> end_link 的变换参数(写死的一个参数)
translation_base_to_end = np.array([-0.073, 0.007, 0.072])  # [x, y, z] (m)
quaternion_base_to_end = [0.655, 0.590, -0.316, -0.349]       # [x, y, z, w]

# 构建齐次变换矩阵 T_hand_eye (end_link->camera)
rotation = R.from_quat(quaternion).as_matrix()
T_hand_eye = np.eye(4)
T_hand_eye[:3, :3] = rotation
T_hand_eye[:3, 3] = translation
camera_link_to_TCP_link = np.linalg.inv(T_hand_eye)  # 求逆矩阵，用于坐标变换
print(f"T_hand_eye: \n {T_hand_eye}")

# 构建旋转矩阵（注意：ROS四元数格式为 [x, y, z, w]）
rotation_base_to_end = R.from_quat(quaternion_base_to_end).as_matrix()
# 构建齐次变换矩阵 T_base_to_end
T_base_to_end = np.eye(4)
T_base_to_end[:3, :3] = rotation_base_to_end
T_base_to_end[:3, 3] = translation_base_to_end
# 步骤2: end_link -> base_link
T_end_to_base = np.linalg.inv(T_base_to_end)
print(f"T_base_to_end: \n {T_base_to_end}")
print(f"T_end_to_base: \n {T_end_to_base}")

def get_aligned_images():  # 定义一个获取图像帧的函数，返回深度和彩色数组
    frames = pipeline.wait_for_frames()  # 等待获取图像帧
    aligned_frames = align.process(frames)  # 获取对齐帧，将深度框与颜色框对齐
    depth_frame = aligned_frames.get_depth_frame()  # 获取深度帧
    color_frame = aligned_frames.get_color_frame()  # 获取对齐帧中的的color帧
    depth_image = np.asanyarray(depth_frame.get_data())  # 将深度帧转换为NumPy数组
    color_image = np.asanyarray(color_frame.get_data())  # 将彩色帧转化为numpy数组

    # 获取深度内参
    depth_intri = depth_frame.profile.as_video_stream_profile().intrinsics

    # cv2.applyColorMap（）将深度图像转化为彩色图像，以便更好的可视化分析
    depth_colormap = cv2.applyColorMap(
        cv2.convertScaleAbs(depth_image, alpha=0.07), cv2.COLORMAP_JET
    )
    # 返回深度内参、对齐深度帧、彩色图像
    return depth_intri, depth_frame, color_image

def compute_target_pose_in_base_link(ux, uy, distance, depth_intri, T_base_to_end, T_hand_eye):
    """
    Given a pixel (ux, uy) and the distance at that pixel, compute the 3D pose of the target
    in the base link frame.
    """

    # 1) Deproject the pixel (ux, uy) to 3D camera coordinates
    camera_xyz = rs.rs2_deproject_pixel_to_point(depth_intri, (ux, uy), distance)

    # 2) Adjust for any sign or orientation flips your setup requires
    camera_xyz = np.array(camera_xyz) * -1

    # 3) Build the 4x4 transform from camera_link to target_link
    camera_link_to_target_link = np.eye(4)
    camera_link_to_target_link[:3, 3] = camera_xyz

    # 4) Compute the pose of the target in base_link frame
    target_link_to_base_link = T_base_to_end @ T_hand_eye @ camera_link_to_target_link

    # 5) Extract translation
    target_pose = {}
    target_pose["x"] = target_link_to_base_link[0, 3]
    target_pose["y"] = target_link_to_base_link[1, 3]
    target_pose["z"] = target_link_to_base_link[2, 3]

    # 6) Extract and convert rotation (3×3 from top-left) to a quaternion using scipy
    rotation_matrix = target_link_to_base_link[:3, :3]
    r = R.from_matrix(rotation_matrix)
    quaternion = r.as_quat()  # Order is [x, y, z, w]

    target_pose["qx"] = quaternion[0]
    target_pose["qy"] = quaternion[1]
    target_pose["qz"] = quaternion[2]
    target_pose["qw"] = quaternion[3]

    return target_pose

if __name__ == '__main__':
    while True:
        # 获取深度帧和彩色帧
        depth_intri, depth_frame, color_image = get_aligned_images()
        source = [color_image]
        # 推理
        results = model.predict(source, save=False)
        with open("detected_objects.json", "w") as f:
            json.dump({"objs":[], "time":time.time()}, f, indent=4)
        # 预测完后处理目标框
        for result in results:
            boxes = result.boxes.xywh.tolist()
            im_array = result.plot()

            for i in range(len(boxes)):  # 遍历boxes列表
                ux, uy = int(boxes[i][0]), int(boxes[i][1])
                dis = depth_frame.get_distance(ux, uy)
                camera_xyz = rs.rs2_deproject_pixel_to_point(depth_intri, (ux, uy), dis)
                camera_xyz = np.round(np.array(camera_xyz), 6) * -1
                camera_xyz = list(camera_xyz)

                cv2.circle(im_array, (ux, uy), 4, (255, 255, 255), 5)  # 标出中心点
                cv2.putText(
                    im_array,
                    str(camera_xyz),
                    (ux + 20, uy + 10),
                    0,
                    0.5,
                    [225, 255, 255],
                    thickness=1,
                    lineType=cv2.LINE_AA
                )  # 标出坐标

                # 计算在 base_link 下的目标姿态并打印
                target_pose = compute_target_pose_in_base_link(
                    ux, uy, dis,
                    depth_intri,
                    T_base_to_end,
                    T_hand_eye
                )
                print(f"target_link_pose:\n{target_pose}")

        cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
        cv2.imshow('RealSense', im_array)
        key = cv2.waitKey(1)
        # Press esc or 'q' to close the image window
        if key & 0xFF == ord('q') or key == 27:
            cv2.destroyAllWindows()
            pipeline.stop()
            break

    # Stop streaming
    pipeline.stop()